﻿#include "CalibrationProcess.h"

using namespace std;
using namespace cv;
using namespace Eigen;

/***************************************************************************************************/
/***全局变量，因为需要将两次标定的数据保存下来使用,这样别的文件包含了这个头文件后就可以直接使用了***/
/***************************************************************************************************/
vector<Mat> cameraMatrix(2, Mat(3, 3, CV_32FC1, Scalar::all(0)));
vector<Mat> distCoeffs(2, Mat(3, 3, CV_32FC1, Scalar::all(0)));//摄像机的5个畸变系数k1,k2,p1,p2,k3
vector<vector<Mat> > tvecsMat(2);//每幅图像的 改正-》平移 向量
vector<vector<Mat> > rvecsMat(2);//每幅图像的 改正-》旋转 向量
vector<vector<MatrixXd> > rotation_matrix(2);


////////////////////////////////////////////////////
int picturenum = 0;
int saveover = 0;
int startsave = 0;
/***************************************************************************************************/

void CalibrationProcess::Init()
{
	char select;
	cout << "是否开始采集图片，请输入Y/N..." << endl;
	cin >> select;
	if (select == 'Y' || select == 'y')
	{
		Pictures.clear();
		cout << "默认每隔5秒采集一次图片，是否想更改采集时间间隔？Y/N" << endl;
		cin >> select;
		if (select == 'y' || select == 'Y')
		{
			cout << "请设定时间间隔 ：" << endl;
			cin >> intervaltime;
		}
		cout << "每隔 " << intervaltime << " s采集一次图像" << endl;
	}
}

bool CalibrationProcess::CollectCalibration(cv::Mat picture)
{
	//是否要进行再次采集
	if (exetime == 0)
	{
		Init();
		exetime = 1;
	}
       CollectPicture(picture);
	if (startCalibration)
	{
		cout << "采集图像完成，共采集" << Pictures.size() << "张图片" << endl;
		cout << "开始处理采集的图像，第一次标定..." << endl;
		CalibrationPicture(Pictures ,ORIGPICTURE);
		cout << "第一次标定完成" << endl;

		cout << "开始矫正..." << endl;
		CorPictures.clear();
		GetCorpictures();
		cout << "矫正完成" << endl;

		cout << "第二次标定..." << endl;
		CalibrationPicture(CorPictures, CORRPICTURE);
		cout << "第二次标定完成" << endl;

		cout << "开始测试结果....." << endl;

		testResult(CORRPICTURE);
		cout << "结果测试结束." << endl;

        startCalibration = 0;

		return true;
	}

	return false;
}

void CalibrationProcess::CollectPicture(cv::Mat picture)
{
	if (exetime == 1)
	{
	        //线程
	    pthread_t tid;
		ret = pthread_create(&tid, NULL, TimeCount, (void*)&intervaltime);
		if (ret == 0)
			cout << "计时器启动.." << endl;
		exetime = 2;
	}
	if (Pictures.size() < COLLECTNUM && exetime == 2)
	{
		namedWindow("Change Picture");
		imshow("Change Picture", picture);
		waitKey(30);

		if (startsave)
		{
			picturenum++;
			cout << "开始保存..." << endl;
			Pictures.push_back(picture.clone());
			imageFileName.clear();
            StrStm.clear();
			StrStm << picturenum;
			StrStm >> imageFileName;
			imageFileName += "_dd.jpg";
			imwrite(imageFileName,Pictures.at(Pictures.size() - 1));
			cout << "保存第" << picturenum << "图像" << endl;

			saveover = 0;
			startsave = 0;
		}
	}
	if (Pictures.size() == COLLECTNUM && exetime == 2)
	{
		picturenum++;
		exetime = 3;
		for (int i = 0; i < Pictures.size(); i++)
		{
			namedWindow("Collect Picture");
			imshow("Collect Picture", Pictures[i]);
			waitKey(500);
			destroyWindow("Collect Picture");
		}
               // destroyWindow("Collect Picture");
		cout << "是否要重新采集？Y/N" << endl;

		char select;
		cin >> select;
		if (select == 'y' || select == 'Y')
		{

			restart();
/*			exetime = 0;
            picturenum = 0;
            Pictures.clear();
            intervaltime = 5;*/
                        
		}
		else
		{
			startCalibration = 1;
            destroyWindow("Change Picture");
		}
	}
}

void CalibrationProcess::CalibrationPicture(vector<Mat> &pictures, int picflag)
{
	Size ImageSize;//图片尺寸
	Size BoardSize = Size(6, 9);//标定板上每行、每列的角点数
	//vector<Point2f> image_points_buf;//缓存每幅图像上检测到的角点	
	vector<int> correctpoints;
    //correctPoint.clear();
	int AngleCount = -1;//用于存储角点数
	int PictureNum = pictures.size();
	int PicIndex = 0;
	Mat Romatrix = Mat(3, 3, CV_32FC1, Scalar::all(0));//保存每幅图像的旋转矩阵

    image_points_buf.clear();
	object_points.clear();
	image_points_seq.clear();

	cout << "开始提取角点....." << endl;

	ImageSize.width = pictures[0].cols;
	ImageSize.height = pictures[0].rows;
	cout << "ImageSize.width = " << ImageSize.width << endl;
	cout << "ImageSize.height = " << ImageSize.height << endl;


	for (int i = 0; i < pictures.size(); i++)
	{


		//提取角点
		if (0 == findChessboardCorners(pictures[i], BoardSize, image_points_buf))
		{
			cout << "can not find chessboard corners " << i << " !\n";
			//exit(1);
		}
		else
		{
			correctpoints.push_back(i);//save correct point 
			Mat view_gray;
			cvtColor(pictures[i], view_gray, CV_BGR2GRAY);
			find4QuadCornerSubpix(view_gray, image_points_buf, Size(10, 10));//亚像素精确化
			image_points_seq.push_back(image_points_buf);//保存亚像素点

			//图像上显示角点位置

			drawChessboardCorners(view_gray, BoardSize, image_points_buf, false);



			namedWindow("Canmera Calibration");
			imshow("Canmera Calibration", view_gray);
			waitKey(500);
			destroyWindow("Canmera Calibration");
		}

	}
	
    correctPoint.push_back(correctpoints);
    cout << "correctPoint.size()" << correctPoint.size() << endl;
	cout << "total = " << image_points_seq.size() << endl;
	cout << endl << "角点提取完成！" << endl;

	//以下是摄像机标定
	cout << "开始标定....." << endl;
	//棋盘三维信息
	Size square_size = Size(26, 26);//实际测量得到的标定板上每个棋盘格的大小
	//vector<vector<Point3f>> object_points;

	vector<int> point_counts;//每幅图像中角点的数量

	//初始化标定板上角点的三维坐标
	int i, j, t;
	//for (t = 0; t < pictures.size(); t++)
	for (t = 0; t < correctPoint[picflag].size(); t++)
	{
		vector<Point3f> tempPointSet;
		for (i = 0; i < BoardSize.height; i++)
		{
			for (j = 0; j < BoardSize.width; j++)
			{
				Point3f realPoint;
				//假设标定板放在世界坐标系中Z=0的平面上
				realPoint.x = i * square_size.width;
				realPoint.y = j * square_size.height;
				realPoint.z = 0;
				tempPointSet.push_back(realPoint);
			}
		}
		object_points.push_back(tempPointSet);
	}
	//初始化每幅图像中的角点数量，假定每幅图像中都可以看到完整的标定板
	//for (i = 0; i < pictures.size(); i++)
	for (i = 0; i < correctPoint[picflag].size(); i++)
	{
		point_counts.push_back(BoardSize.width*BoardSize.height);
	}
	//开始标定
	calibrateCamera(object_points, image_points_seq, ImageSize, cameraMatrix[picflag], distCoeffs[picflag], rvecsMat[picflag], tvecsMat[picflag], 0);
	cout << "标定完成~！" << endl;

	//对标定结果进行评价
	cout << "开始评价标定结果......" << endl;
	double total_err = 0.0;//所有图像的平均误差的总和
	double err = 0.0;//每幅图像的平均误差
	vector<Point2f> image_points2;//保存重新计算得到的投影点
	cout << "\t每幅图像的标定误差：" << endl;
	//for (i = 0; i < pictures.size(); i++)
	for (i = 0; i < correctPoint[picflag].size(); i++)
	{
		vector<Point3f> tempPointSet = object_points[i];
		//通过得到的摄像机内外参数，对空间的三维点进行重新投影计算,rvecsMat平移向量
		projectPoints(tempPointSet, rvecsMat[picflag][i], tvecsMat[picflag][i], cameraMatrix[picflag], distCoeffs[picflag], image_points2);
		//计算心得投影点和旧的投影点之间的误差
		vector<Point2f> tempImagePoint = image_points_seq[i];
		Mat tempImagePointMat = Mat(1, tempImagePoint.size(), CV_32FC2);
		Mat image_points2Mat = Mat(1, image_points2.size(), CV_32FC2);
		for (int j = 0; j < tempImagePoint.size(); j++)
		{
			image_points2Mat.at<Vec2f>(0, j) = Vec2f(image_points2[j].x, image_points2[j].y);
			tempImagePointMat.at<Vec2f>(0, j) = Vec2f(tempImagePoint[j].x, tempImagePoint[j].y);
		}
		err = norm(image_points2Mat, tempImagePointMat, NORM_L2);
		total_err += err /= point_counts[i];
		cout << "第" << i + 1 << "幅图像的平均误差：" << err << "像素" << endl;
	}
//	cout << "总体平均误差：" << total_err / pictures.size() << "像素" << endl;
	cout << "总体平均误差：" << total_err / correctPoint[picflag].size() << "像素" << endl;
	cout << "评价完成!" << endl;

}

Eigen::MatrixXd CalibrationProcess::CalculateHmatrix(int imageNum, int picflag)
{
	MatrixXd InMatrix(3, 3);
	MatrixXd OutMatrix(3, 4);
	MatrixXd HMatrix(3, 4);

	for (int i = 0; i < 3; i++)
	{
		for (int j = 0; j < 3; j++)
		{
			InMatrix(i, j) = cameraMatrix[picflag].at<double>(i, j);
		}
	}
	for (int i = 0; i < correctPoint[picflag].size(); i++)
	{
		Rodrigues(rvecsMat[picflag][i], Romatrix);

		for (int i = 0; i < 3; i++)
		{
			for (int j = 0; j < 3; j++)
			{
				OutMatrix(i, j) = Romatrix.at<double>(i, j);

			}
		}
		OutMatrix(0, 3) = tvecsMat[picflag][i].at<double>(0);
		OutMatrix(1, 3) = tvecsMat[picflag][i].at<double>(1);
		OutMatrix(2, 3) = tvecsMat[picflag][i].at<double>(2);

		rotation_matrix[picflag].push_back(OutMatrix);
	}

	HMatrix = InMatrix * rotation_matrix[picflag][imageNum];

	return HMatrix;
}

cv::Mat CalibrationProcess::CorrectPicture(cv::Mat frame, bool display)
{
	cout << "矫正图片...." << endl;
	//Size BoardSize = Size(6, 9);//标定板上每行、每列的角点数
	Size image_size;


	Mat correctFrame = frame.clone();
	Mat correct = frame.clone();
	image_size.width = correctFrame.cols;
	image_size.height = correctFrame.rows;
	Mat mapx = Mat(image_size, CV_32FC1);
	Mat mapy = Mat(image_size, CV_32FC1);
	Mat R = Mat();//Mat::eye(3, 3, CV_32F);


	distCoeffs[0].at<double>(0, 4) = 0.0;
	initUndistortRectifyMap(cameraMatrix[0], distCoeffs[0], R, getOptimalNewCameraMatrix(cameraMatrix[0], distCoeffs[0], image_size, 1, image_size, 0), image_size, CV_32FC1, mapx, mapy);
	remap(correctFrame, correct, mapx, mapy, CV_INTER_LINEAR);

    if(display == true)
	{

		circle(correctFrame, image_points_seq[pointnum][0], 5, Scalar(0,0,255));
		circle(correctFrame, image_points_seq[pointnum][1], 5, Scalar(0,0,255));
		pointnum++;

		namedWindow("correctFrame");
		imshow("correctFrame", correctFrame);
		waitKey(500);
	    destroyWindow("correctFrame");

	    char ass;
	    cin >> ass;


		namedWindow("correct");
		imshow("correct", correct);
		waitKey(500);
	    destroyWindow("correct");
	}

	/*imageFileName.clear();
	StrStm.clear();
	StrStm << ++croNum;
	StrStm >> imageFileName;
	imageFileName += "_d.jpg";
	imwrite(imageFileName, correct);*/
   
	return correct;
}

cv::Point2d CalibrationProcess::TableMap(float X, float Y, Eigen::MatrixXd HMatrix)
{
	Point2d result;


	a1 = HMatrix(0, 0) - HMatrix(2, 0) * X;
	a2 = HMatrix(0, 1) - HMatrix(2, 1) * X;
	a3 = HMatrix(0, 2) - HMatrix(2, 2) * X - HMatrix(2, 3) * X + HMatrix(0, 3);
	a4 = HMatrix(1, 0) - HMatrix(2, 0) * Y;
	a5 = HMatrix(1, 1) - HMatrix(2, 1) * Y;
	a6 = HMatrix(1, 2) - HMatrix(2, 2) * Y - HMatrix(2, 3) * Y + HMatrix(1, 3);

	result.y = (a3 * a4 - a1 * a6) / (a1 * a5 - a2 * a4);
	result.x = -1 * (a2 * result.y + a3) / a1;

	//cout << "a1" << a1 << " a2" << a2 << " a3" << a3 << " a4" << a4 << " a5" << a5 << " a6" << a6 << endl;

	return result;
}

void * CalibrationProcess::TimeCount(void * args)
{

	int i = *((int*)args);
	while (picturenum < (COLLECTNUM+1))
	{
		if (!saveover)
		{
			sleep(i);
			startsave = 1;
			saveover = 1;
		}
	}
	cout << "关闭计时器" << endl;
	picturenum = 0;
        startsave = 0;
        saveover = 0;
}

void CalibrationProcess::testResult(int picflag)
{
	bool judge;
	MatrixXd HMatrix;
	for (int j = 0; j < correctPoint[picflag].size(); j++)
	{
		HMatrix = CalculateHmatrix(j, picflag);
		//coutH(HMatrix);
		cout << "第" << j + 1 << "幅图像的误差：" << endl;
		for (int i = 0; i < 54; i++)
		{
			Point2d result;
			result = TableMap(image_points_seq[j][i].x, image_points_seq[j][i].y, HMatrix);
			cout << "X err " << result.x - object_points[j][i].x;
			cout << "  Y err " << result.y - object_points[j][i].y << endl;
			judge = false;
		}
	}
}

void CalibrationProcess::GetCorpictures()
{
	pointnum = 0;
	for (int i = 0; i < correctPoint[0].size(); i++)
	{
		CorPictures.push_back(CorrectPicture(Pictures[correctPoint[0][i]].clone(), true));
	}
	pointnum = 0;
	
}

std::vector<double> CalibrationProcess::PixelMapWorld(std::vector<double>& Pixel, int picturetime)
{
    MatrixXd HMatrix;
    HMatrix = CalculateHmatrix(picturetime, CORRPICTURE);
    vector<double> result;
    //coutH(HMatrix);
	a1 = HMatrix(0, 0) - HMatrix(2, 0) * Pixel[0];
	a2 = HMatrix(0, 1) - HMatrix(2, 1) * Pixel[0];
	a3 = HMatrix(0, 2) - HMatrix(2, 2) * Pixel[0] - HMatrix(2, 3) * Pixel[0] + HMatrix(0, 3);
	a4 = HMatrix(1, 0) - HMatrix(2, 0) * Pixel[1];
	a5 = HMatrix(1, 1) - HMatrix(2, 1) * Pixel[1];
	a6 = HMatrix(1, 2) - HMatrix(2, 2) * Pixel[1] - HMatrix(2, 3) * Pixel[1] + HMatrix(1, 3);

	result.push_back((a3 * a4 - a1 * a6) / (a1 * a5 - a2 * a4));
	result.push_back(-1 * (a2 * result[0] + a3) / a1);

	//cout << "a1" << a1 << " a2" << a2 << " a3" << a3 << " a4" << a4 << " a5" << a5 << " a6" << a6 << endl;

     return result;
}

void CalibrationProcess::coutH(MatrixXd HMatrix)
{

	for(int i = 0; i < 3; i ++)
	{
		for(int j = 0; j < 4; j++)
		{
			cout << HMatrix(i,j) << " ";
		}
		cout << endl;
	}
	cout << endl;
}


void CalibrationProcess::restart()
{

			exetime = 0;
            picturenum = 0;
            Pictures.clear();
            intervaltime = 5;
}